#include "stdafx.h"
#include "ExampleCGAL.h"

EXAMPLECGAL_API SAFEARRAY * CALLBACK skeleton(SAFEARRAY ** point_array, bool withAllBisector)
{
	Polygon_2 poly=safe2poly(point_array);

	SAFEARRAY *out=NULL;
	if(poly.size()>0){
		if(!poly.is_simple())
			return out;

		if(!poly.is_counterclockwise_oriented())
			poly.reverse_orientation();

		SharedPointer iss=CGAL::create_interior_straight_skeleton_2(
														poly.vertices_begin(),
														poly.vertices_end()
														);
		StraightSkeleton const& ss = *iss;

		He_handle he;
		V_handle v;
		Point_vector pv;
		Segment_vector sv;

		for(He_iterator i=ss.halfedges_begin(); i!=ss.halfedges_end(); ++i){
			if(i->is_inner_bisector() || (withAllBisector && i->is_bisector())){
				Segment s(i->opposite()->vertex()->point(),i->vertex()->point());
				if(std::find(sv.begin(),sv.end(),s) == sv.end() && 
					std::find(sv.begin(),sv.end(),s.opposite()) == sv.end())
					sv.push_back(s);
			}
		}

		SAFEARRAYBOUND b[2];
		b[0].lLbound = 0;
		b[1].lLbound = 0;
		b[0].cElements=sv.size();
		b[1].cElements=4;

		CComSafeArray<double,VT_R8> skeleton_points(b,2);

		long idx[2]={0,0};
		for(Segment_iterator i=sv.begin(); i!=sv.end(); ++i){
			if(S_OK != skeleton_points.MultiDimSetAt(idx,i->source().x())){
				skeleton_points.Destroy();
				break;
			}
			idx[1]++;
			if(S_OK != skeleton_points.MultiDimSetAt(idx,i->source().y())){
				skeleton_points.Destroy();
				break;
			}
			idx[1]++;
			if(S_OK != skeleton_points.MultiDimSetAt(idx,i->target().x())){
				skeleton_points.Destroy();
				break;
			}
			idx[1]++;
			if(S_OK != skeleton_points.MultiDimSetAt(idx,i->target().y())){
				skeleton_points.Destroy();
				break;
			}
			idx[0]++;
			idx[1]=0;
		}
		out=skeleton_points.Detach();
		skeleton_points.Destroy();

	}
	return out;
}

EXAMPLECGAL_API SAFEARRAY * CALLBACK simplify(SAFEARRAY ** point_array, double fract, double mindist, bool convexHull)
{
	int iter=0;
	Point_vector pv;
	Polygon_2 poly=safe2poly(point_array);

	if(!convexHull){
		//do{
			Vp_circulator vc=poly.vertices_circulator();
			pv.clear();
			Point ini=*vc;

			do{
				if(angle_3points(*vc, *(vc+1), *(vc+2)) < PI/fract)
					pv.push_back(*(vc+1));
				++vc;
			}while(*vc != ini);

		//	iter++;
		//	fract+=FRACT_STEP;
		//}while(poly.size()-pv.size() < 4 && iter < MAX_ITER);

		if(poly.size()-pv.size() >= 4){
			for(Point_iterator i=pv.begin(); i != pv.end(); ++i){
				Vp_iterator vp=std::find(poly.vertices_begin(),poly.vertices_end(),*i);
				if(vp != poly.vertices_end())
					poly.erase(vp);
			}
			remove_with_dist(poly,mindist);
		}
		
	}else
		poly=convex_hull(poly);

	return poly2safe(poly);
}

EXAMPLECGAL_API void remove_with_dist(Polygon_2 &poly, double dist)
{
	int iter=0;
	Point_vector pv;

	//do{
		pv.clear();
		Vp_circulator i=poly.vertices_circulator();
		Point ini=*i;

		do{
			if(CGAL::sqrt(Segment(*i,*(i+1)).squared_length()) < dist)
				pv.push_back(*i);
			++i;
		}while(*i != ini);

	//TODO
	//	iter++;
	//	dist+=DIST_STEP;
	//}while(poly.size()-pv.size() < 4 && iter < MAX_ITER);

	if(poly.size()-pv.size() >= 4)
		for(Point_iterator i=pv.begin(); i != pv.end(); ++i){
			Vp_iterator vp=std::find(poly.vertices_begin(),poly.vertices_end(),*i);
			if(vp != poly.vertices_end())
				poly.erase(vp);
		}
}

EXAMPLECGAL_API double angle_Xaxis(Point p1, Point p2)
{
	Segment s1(p1, p2);
	double out=std::atan2(s1.direction().dy(),s1.direction().dx());

	return (out >= 0) ? out : 2*PI+out;
}

EXAMPLECGAL_API double angle_Xaxis(Segment s1)
{
	return angle_Xaxis(s1.source(),s1.target());
}

EXAMPLECGAL_API double angle_3points(Point p1, Point p2,Point p3)
{
	return CGAL::abs(angle_Xaxis(p1,p2)-angle_Xaxis(p2,p3));
}

EXAMPLECGAL_API double angle_2segments(Segment s1,Segment s2)
{
	return CGAL::abs(angle_Xaxis(s1)-angle_Xaxis(s2));
}

EXAMPLECGAL_API Polygon_2 convex_hull(Polygon_2 poly)
{
	Point_vector ch_result;
	CGAL::convex_hull_2(poly.vertices_begin(),poly.vertices_end(),std::back_inserter(ch_result));
	return Polygon_2(ch_result.begin(),ch_result.end());
}

EXAMPLECGAL_API Polygon_2 safe2poly(SAFEARRAY ** point_array)
{
	CComSafeArray<double,VT_R8> points;
	points.Attach(*point_array);

	Polygon_2 poly;
	for(long i=0; i < points.GetCount(0); i++){
		long idx[2]={i,0};

		double x,y;
		points.MultiDimGetAt(idx,x);
		idx[1]++;
		points.MultiDimGetAt(idx,y);

		poly.push_back(Point(x,y));
	}

	points.Detach();
	points.Destroy();
	return poly;
}

EXAMPLECGAL_API SAFEARRAY * poly2safe(Polygon_2 poly)
{
	
	SAFEARRAYBOUND b[2];
	b[0].lLbound = 0;
	b[1].lLbound = 0;
	b[0].cElements=poly.size();
	b[1].cElements=2;
	
	CComSafeArray<double,VT_R8> points(b,2);
	
	long idx[2]={0,0};
	for(Vp_iterator i=poly.vertices_begin(); i!=poly.vertices_end(); ++i){
		if(S_OK != points.MultiDimSetAt(idx,i->x())){
			points.Destroy();
			break;
		}
		idx[1]++;
		if(S_OK != points.MultiDimSetAt(idx,i->y())){
			points.Destroy();
			break;
		}
		idx[0]++;
		idx[1]=0;
	}

	SAFEARRAY *out;
	out=points.Detach();
	points.Destroy();
	return out;
}

//for(V_iterator i=ss.vertices_begin(); i!=ss.vertices_end(); ++i){
//	if(i->is_skeleton())
//		if(std::find(pv.begin(),pv.end(),i->point()) == pv.end())
//			pv.push_back(i->point());
//}

//for(He_iterator i=ss.halfedges_begin(); i!=ss.halfedges_end(); ++i){
//	if(i->is_inner_bisector() || (withAllBisector && i->is_bisector())){
//		if(std::find(pv.begin(),pv.end(),i->opposite()->vertex()->point()) == pv.end())
//			pv.push_back(i->opposite()->vertex()->point());
//		if(std::find(pv.begin(),pv.end(),i->vertex()->point()) == pv.end())
//			pv.push_back(i->vertex()->point());
//	}
//}

//std::sort(pv.begin(),pv.end());
//Point_iterator pv_it=std::unique(pv.begin(),pv.end());
//pv.resize(std::distance(pv.begin(),pv_it));